An Efficient Motion Planning Algorithm for Stochastic Dynamic Systems with Constraints on Probability of Failure

When controlling dynamic systems, such as mobile robots in uncertain environments, there is a trade off between risk and reward. For example, a race car can turn a corner faster by taking a more challenging path. This paper proposes a new approach to planning a control sequence with a guaranteed risk bound. Given a stochastic dynamic model, the problem is to find a control sequence that optimizes a performance metric, while satisfying chance constraints i.e. constraints on the upper bound of the probability of failure. We propose a two-stage optimization approach, with the upper stage optimizing the risk allocation and the lower stage calculating the optimal control sequence that maximizes reward. In general, the upper-stage is a non-convex optimization problem, which is hard to solve. We develop a new iterative algorithm for this stage that efficiently computes the risk allocation with a small penalty to optimality. The algorithm is implemented and tested on the autonomous underwater vehicle (AUV) depth planning problem, and demonstrates a substantial improvement in computation cost and suboptimality, compared to the prior arts. Introduction Physically grounded AI systems typically interact with their environment through a hybrid of discrete and continuous actions. Two important capabilities for such systems are kinodynamic motion planning and plan execution on a hybrid discrete/continuous plant. For example, our application is a bathymetric mapping mission using Dorado-class autonomous underwater vehicle (AUV) (Figure 1) operated by the Monterey Bay Aquarium Research Institute (MBARI). Dorado-class mapping AUV is 6,000 m rated, and can operate for up to 20 hours without human supervision. This system should ideally navigate itself to areas of scientific interest, such as underwater canyons, according to a game plan provided by scientists. Since the AUV’s maneuverability is limited, it needs to plan its path while taking vehicle dynamics into account, in order to avoid collisions with the seafloor. A model-based executive, called Sulu (Léauté ∗This research is funded by The Boeing Company grant MITBA-GTA-1 Copyright c © 2008, Association for the Advancement of Artificial Intelligence (www.aaai.org). All rights reserved. Figure 1: Dorado-class autonomous underwater vehicle of Monterey Bay Aquarium Research Institute 2005), implemented these two capabilities using deterministic model. Real-world systems can be exposed to significant levels of stochastic disturbance. Stochastic systems typically have a risk of failure due to unexpected events, such as unpredictable tides and currents that affect the AUV’s motion. To reduce the risk of failure, the AUV needs to stay away from failure states, such as the seafloor. This has the consequence of reducing mission performance, since it prohibits high resolution observation of the seafloor. Thus operators of stochastic systems need to trade-off risk with performance. A common approach to trading-off risk and performance is to define a positive reward for mission achievement and a negative reward for failure, and then optimize the expected reward using a Markov Decision Process (MDP) encoding. However, in many practical cases, only an arbitrary definition of reward is possible. For example, it is hard to define the value of scientific discovery compared to the cost of losing the AUV. Another approach to trading off risk and performance is to limit the probability of failure (chance constraint) and maximize the performance under this constraint. For example, an AUV minimizes the average altitude from the seafloor, while limiting the probability of collision to 0.1%. There is a considerable body of work on this approach within Robust Model Predictive Control (RMPC) community. If the distribution of disturbance is bounded, zero failure probability can be achieved by sparing the safety margin between the failure states and the nominal states (Kuwata, Proceedings of the Twenty-Third AAAI Conference on Artificial Intelligence (2008)